Fitting Polynomials

As we learned in the previous video, the reference trajectory is typically passed to the control block as a polynomial. This polynomial is usually 3rd order, since third order polynomials will fit trajectories for most roads. To practice this most common situation, we will learn how to fit 3rd order polynomials to waypoints (x, y) in C++ using Eigen, and evaluate the output. Your tasks are:

Use polyfit to fit a 3rd order polynomial to the given x and y coordinates representing waypoints.
Use polyeval to evaluate y values of given x coordinates.

Start Quiz:

main.cpp

// In this quiz you'll fit a polynomial to waypoints.

#include <iostream>
#include "Dense"

using namespace Eigen;

// Evaluate a polynomial.
double polyeval(Eigen::VectorXd coeffs, double x) {
  double result = 0.0;
  for (int i = 0; i < coeffs.size(); i++) {
    result += coeffs[i] * pow(x, i);
  }
  return result;
}

// Fit a polynomial.
// Adapted from
// https://github.com/JuliaMath/Polynomials.jl/blob/master/src/Polynomials.jl#L676-L716
Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals,
                        int order) {
  assert(xvals.size() == yvals.size());
  assert(order >= 1 && order <= xvals.size() - 1);
  Eigen::MatrixXd A(xvals.size(), order + 1);

  for (int i = 0; i < xvals.size(); i++) {
    A(i, 0) = 1.0;
  }

  for (int j = 0; j < xvals.size(); j++) {
    for (int i = 0; i < order; i++) {
      A(j, i + 1) = A(j, i) * xvals(j);
    }
  }

  auto Q = A.householderQr();
  auto result = Q.solve(yvals);
  return result;
}

int main() {
  Eigen::VectorXd xvals(6);
  Eigen::VectorXd yvals(6);
  // x waypoint coordinates
  xvals << 9.261977, -2.06803, -19.6663, -36.868, -51.6263, -66.3482;
  // y waypoint coordinates
  yvals << 5.17, -2.25, -15.306, -29.46, -42.85, -57.6116;

  // TODO: use `polyfit` to fit a third order polynomial to the (x, y)
  // coordinates.
  // Hint: call Eigen::VectorXd polyfit() and pass xvals, yvals, and the 
  // polynomial degree/order
  // YOUR CODE HERE

  for (double x = 0; x <= 20; x += 1.0) {
    // TODO: use `polyeval` to evaluate the x values.
    std::cout << "YOUR CODE HERE" << std::endl; 
  }

  // Expected output
  // -0.905562
  // -0.226606
  // 0.447594
  // 1.11706
  // 1.7818
  // 2.44185
  // 3.09723
  // 3.74794
  // 4.39402
  // 5.03548
  // 5.67235
  // 6.30463
  // 6.93236
  // 7.55555
  // 8.17423
  // 8.7884
  // 9.3981
  // 10.0033
  // 10.6041
  // 11.2005
  // 11.7925
}